home *** CD-ROM | disk | FTP | other *** search
/ io Programmo 60 / IOPROG_60.ISO / soft / c++ / gsl-1.1.1-setup.exe / {app} / src / ode-initval / rkf45.c < prev    next >
Encoding:
C/C++ Source or Header  |  2002-04-18  |  7.8 KB  |  324 lines
  1. /* ode-initval/rkf45.c
  2.  * 
  3.  * Copyright (C) 2001 Brian Gough
  4.  * 
  5.  * This program is free software; you can redistribute it and/or modify
  6.  * it under the terms of the GNU General Public License as published by
  7.  * the Free Software Foundation; either version 2 of the License, or (at
  8.  * your option) any later version.
  9.  * 
  10.  * This program is distributed in the hope that it will be useful, but
  11.  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12.  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13.  * General Public License for more details.
  14.  * 
  15.  * You should have received a copy of the GNU General Public License
  16.  * along with this program; if not, write to the Free Software
  17.  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  18.  */
  19.  
  20. /* Runge-Kutta-Fehlberg 4(5)*/
  21.  
  22. #include <config.h>
  23. #include <stdlib.h>
  24. #include <string.h>
  25. #include <gsl/gsl_errno.h>
  26. #include <gsl/gsl_odeiv.h>
  27.  
  28. #include "odeiv_util.h"
  29.  
  30. /* Runge-Kutta-Fehlberg constants */
  31. static const double ah[] = { 1.0/4.0, 3.0/8.0, 12.0/13.0, 1.0, 1.0/2.0 };
  32.  
  33. static const double b3[] = { 3.0/32.0, 9.0/32.0 };
  34.  
  35. static const double b4[] = { 1932.0/2197.0, -7200.0/2197.0, 7296.0/2197.0};
  36.  
  37. static const double b5[] = { 8341.0/4104.0, -32832.0/4104.0, 29440.0/4104.0, -845.0/4104.0};
  38.  
  39. static const double b6[] = { -6080.0/20520.0, 41040.0/20520.0, -28352.0/20520.0, 9295.0/20520.0, -5643.0/20520.0};
  40.  
  41. static const double c1 = 902880.0/7618050.0;
  42. static const double c3 = 3953664.0/7618050.0;
  43. static const double c4 = 3855735.0/7618050.0;
  44. static const double c5 = -1371249.0/7618050.0;
  45. static const double c6 = 277020.0/7618050.0;
  46.  
  47.  
  48. static const double ec[] = { 0.0,
  49.   1.0 / 360.0,
  50.   0.0,
  51.   -128.0 / 4275.0,
  52.   -2197.0 / 75240.0,
  53.   1.0 / 50.0,
  54.   2.0 / 55.0
  55. };
  56.  
  57. typedef struct
  58. {
  59.   double *k1;
  60.   double *k2;
  61.   double *k3;
  62.   double *k4;
  63.   double *k5;
  64.   double *k6;
  65.   double *y0;
  66.   double *ytmp;
  67. }
  68. rkf45_state_t;
  69.  
  70. static void *
  71. rkf45_alloc (size_t dim)
  72. {
  73.   rkf45_state_t *state = (rkf45_state_t *) malloc (sizeof (rkf45_state_t));
  74.  
  75.   if (state == 0)
  76.     {
  77.       GSL_ERROR_NULL ("failed to allocate space for rkf45_state", GSL_ENOMEM);
  78.     }
  79.  
  80.   state->k1 = (double *) malloc (dim * sizeof (double));
  81.  
  82.   if (state->k1 == 0)
  83.     {
  84.       free (state);
  85.       GSL_ERROR_NULL ("failed to allocate space for k1", GSL_ENOMEM);
  86.     }
  87.  
  88.   state->k2 = (double *) malloc (dim * sizeof (double));
  89.  
  90.   if (state->k2 == 0)
  91.     {
  92.       free (state->k1);
  93.       free (state);
  94.       GSL_ERROR_NULL ("failed to allocate space for k2", GSL_ENOMEM);
  95.     }
  96.  
  97.   state->k3 = (double *) malloc (dim * sizeof (double));
  98.  
  99.   if (state->k3 == 0)
  100.     {
  101.       free (state->k2);
  102.       free (state->k1);
  103.       free (state);
  104.       GSL_ERROR_NULL ("failed to allocate space for k3", GSL_ENOMEM);
  105.     }
  106.  
  107.   state->k4 = (double *) malloc (dim * sizeof (double));
  108.  
  109.   if (state->k4 == 0)
  110.     {
  111.       free (state->k3);
  112.       free (state->k2);
  113.       free (state->k1);
  114.       free (state);
  115.       GSL_ERROR_NULL ("failed to allocate space for k4", GSL_ENOMEM);
  116.     }
  117.  
  118.   state->k5 = (double *) malloc (dim * sizeof (double));
  119.  
  120.   if (state->k5 == 0)
  121.     {
  122.       free (state->k4);
  123.       free (state->k3);
  124.       free (state->k2);
  125.       free (state->k1);
  126.       free (state);
  127.       GSL_ERROR_NULL ("failed to allocate space for k5", GSL_ENOMEM);
  128.     }
  129.  
  130.   state->k6 = (double *) malloc (dim * sizeof (double));
  131.  
  132.   if (state->k6 == 0)
  133.     {
  134.       free (state->k5);
  135.       free (state->k4);
  136.       free (state->k3);
  137.       free (state->k2);
  138.       free (state->k1);
  139.       free (state);
  140.       GSL_ERROR_NULL ("failed to allocate space for k6", GSL_ENOMEM);
  141.     }
  142.  
  143.   state->y0 = (double *) malloc (dim * sizeof (double));
  144.  
  145.   if (state->y0 == 0)
  146.     {
  147.       free (state->k6);
  148.       free (state->k5);
  149.       free (state->k4);
  150.       free (state->k3);
  151.       free (state->k2);
  152.       free (state->k1);
  153.       free (state);
  154.       GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
  155.     }
  156.  
  157.   state->ytmp = (double *) malloc (dim * sizeof (double));
  158.  
  159.   if (state->ytmp == 0)
  160.     {
  161.       free (state->y0);
  162.       free (state->k6);
  163.       free (state->k5);
  164.       free (state->k4);
  165.       free (state->k3);
  166.       free (state->k2);
  167.       free (state->k1);
  168.       free (state);
  169.       GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
  170.     }
  171.  
  172.   return state;
  173. }
  174.  
  175.  
  176. static int
  177. rkf45_apply (void *vstate,
  178.         size_t dim,
  179.         double t,
  180.         double h,
  181.         double y[],
  182.         double yerr[],
  183.         const double dydt_in[],
  184.         double dydt_out[], const gsl_odeiv_system * sys)
  185. {
  186.   rkf45_state_t *state = (rkf45_state_t *) vstate;
  187.  
  188.   size_t i;
  189.   int status = 0;
  190.  
  191.   double *const k1 = state->k1;
  192.   double *const k2 = state->k2;
  193.   double *const k3 = state->k3;
  194.   double *const k4 = state->k4;
  195.   double *const k5 = state->k5;
  196.   double *const k6 = state->k6;
  197.   double *const ytmp = state->ytmp;
  198.  
  199.   /* k1 step */
  200.   if (dydt_in != NULL)
  201.     {
  202.       DBL_MEMCPY (k1, dydt_in, dim);
  203.     }
  204.   else
  205.     {
  206.       int s = GSL_ODEIV_FN_EVAL (sys, t, y, k1);
  207.       GSL_STATUS_UPDATE (&status, s);
  208.     }
  209.  
  210.   for (i = 0; i < dim; i++)
  211.     ytmp[i] = y[i] +  ah[0] * h * k1[i];
  212.  
  213.   /* k2 step */
  214.   {
  215.     int s = GSL_ODEIV_FN_EVAL (sys, t + ah[0] * h, ytmp, k2);
  216.     GSL_STATUS_UPDATE (&status, s);
  217.   }
  218.   for (i = 0; i < dim; i++)
  219.     ytmp[i] = y[i] + h * (b3[0] * k1[i] + b3[1] * k2[i]);
  220.  
  221.   /* k3 step */
  222.   {
  223.     int s = GSL_ODEIV_FN_EVAL (sys, t + ah[1] * h, ytmp, k3);
  224.     GSL_STATUS_UPDATE (&status, s);
  225.   }
  226.   for (i = 0; i < dim; i++)
  227.     ytmp[i] = y[i] + h * (b4[0] * k1[i] + b4[1] * k2[i] + b4[2] * k3[i]);
  228.  
  229.   /* k4 step */
  230.   {
  231.     int s = GSL_ODEIV_FN_EVAL (sys, t + ah[2] * h, ytmp, k4);
  232.     GSL_STATUS_UPDATE (&status, s);
  233.   }
  234.   for (i = 0; i < dim; i++)
  235.     ytmp[i] =
  236.       y[i] + h * (b5[0] * k1[i] + b5[1] * k2[i] + b5[2] * k3[i] +
  237.           b5[3] * k4[i]);
  238.  
  239.   /* k5 step */
  240.   {
  241.     int s = GSL_ODEIV_FN_EVAL (sys, t + ah[3] * h, ytmp, k5);
  242.     GSL_STATUS_UPDATE (&status, s);
  243.   }
  244.   for (i = 0; i < dim; i++)
  245.     ytmp[i] =
  246.       y[i] + h * (b6[0] * k1[i] + b6[1] * k2[i] + b6[2] * k3[i] +
  247.           b6[3] * k4[i] + b6[4] * k5[i]);
  248.  
  249.   /* k6 step and final sum */
  250.   {
  251.     int s = GSL_ODEIV_FN_EVAL (sys, t + ah[4] * h, ytmp, k6);
  252.     GSL_STATUS_UPDATE (&status, s);
  253.   }
  254.  
  255.   for (i = 0; i < dim; i++)
  256.     {
  257.       const double d_i = c1 * k1[i] + c3 * k3[i] + c4 * k4[i] + c5 * k5[i] + c6 * k6[i];
  258.       y[i] += h * d_i;
  259.       if (dydt_out != NULL)
  260.     dydt_out[i] = d_i;
  261.     }
  262.  
  263.   /* difference between 4th and 5th order */
  264.   for (i = 0; i < dim; i++)
  265.     yerr[i] =
  266.       h * (ec[1] * k1[i] + ec[3] * k3[i] + ec[4] * k4[i] + ec[5] * k5[i] +
  267.        ec[6] * k6[i]);
  268.  
  269.   return status;
  270. }
  271.  
  272.  
  273. static int
  274. rkf45_reset (void *vstate, size_t dim)
  275. {
  276.   rkf45_state_t *state = (rkf45_state_t *) vstate;
  277.  
  278.   DBL_ZERO_MEMSET (state->k1, dim);
  279.   DBL_ZERO_MEMSET (state->k2, dim);
  280.   DBL_ZERO_MEMSET (state->k3, dim);
  281.   DBL_ZERO_MEMSET (state->k4, dim);
  282.   DBL_ZERO_MEMSET (state->k5, dim);
  283.   DBL_ZERO_MEMSET (state->k6, dim);
  284.   DBL_ZERO_MEMSET (state->ytmp, dim);
  285.  
  286.   return GSL_SUCCESS;
  287. }
  288.  
  289. static unsigned int
  290. rkf45_order (void *vstate)
  291. {
  292.   rkf45_state_t *state = (rkf45_state_t *) vstate;
  293.   state = 0; /* prevent warnings about unused parameters */
  294.   return 5;
  295. }
  296.  
  297. static void
  298. rkf45_free (void *vstate)
  299. {
  300.   rkf45_state_t *state = (rkf45_state_t *) vstate;
  301.  
  302.   free (state->ytmp);
  303.   free (state->y0);
  304.   free (state->k6);
  305.   free (state->k5);
  306.   free (state->k4);
  307.   free (state->k3);
  308.   free (state->k2);
  309.   free (state->k1);
  310.   free (state);
  311. }
  312.  
  313. static const gsl_odeiv_step_type rkf45_type = { "rkf45",    /* name */
  314.   1,                /* can use dydt_in */
  315.   0,                /* gives exact dydt_out */
  316.   &rkf45_alloc,
  317.   &rkf45_apply,
  318.   &rkf45_reset,
  319.   &rkf45_order,
  320.   &rkf45_free
  321. };
  322.  
  323. const gsl_odeiv_step_type *gsl_odeiv_step_rkf45 = &rkf45_type;
  324.